/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;
import edu.wpi.first.wpilibj.Joystick.ButtonType;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;


/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot3559 extends IterativeRobot {

    //Command autonomousCommand;

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        // instantiate the command used for the autonomous period
        
        NetworkTable.initialize();
        
        //autonomousCommand = new ExampleCommand();

        // Initialize all subsystems
        CommandBase.init();
        SmartDashboard.putData("SchedulerData", Scheduler.getInstance());
    }

    public void autonomousInit() {
        // schedule the autonomous command (example)
        //autonomousCommand.start();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }

    public void teleopInit() {
	// This makes sure that the autonomous stops running when
        // teleop starts running. If you want the autonomous to 
        // continue until interrupted by another command, remove
        // this line or comment it out.        
        //autonomousCommand.cancel();
        
        updateDash();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
        CommandBase.chassis.toTank();
        //SmartDashboard.putDouble("Bridge Arm", CommandBase.bridgearm.bridgeArmStop());
        CommandBase.chassis.updateDash();
        updateDash();
        
       
        if(OI.getInstance().getLeftJoystick().getRawButton(3)) {
            CommandBase.chassis.toTank();
        }
        
    }
    
    public void disabledPeriodic(){
        
    }
    
    public void updateDash() {
            SmartDashboard.putDouble("LeftStick", OI.getInstance().leftJoystick.getY());
            SmartDashboard.putDouble("RightStick", OI.getInstance().rightJoystick.getY());
            SmartDashboard.putDouble("Shooter", (((OI.getInstance().thirdJoystick.getZ()+1))/2));
  
            /*
            SmartDashboard.putBoolean("Lower", EBLS.get());
            SmartDashboard.putBoolean("Middle", EMLS.get());
            SmartDashboard.putBoolean("Top", ETLS.get());
            SmartDashboard.putBoolean("Button", ThirdStick.getRawButton(2));
            SmartDashboard.putBoolean("Back of Arm", ABLS.get());
            SmartDashboard.putBoolean("Front of Arm", AFLS.get());
            SmartDashboard.putBoolean("Tilt Back limit switch", TBLS.get());
             */
    }
}
